/*
 * MotionModel.h
 *
 *  Created on: 16/11/2011
 *      Author: will
 */

#ifndef MOTIONMODEL_H_
#define MOTIONMODEL_H_

#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/LU>


using Eigen::Vector3d;
using Eigen::Matrix3d;

static const double pi = 3.14159265358979323846;

double randn(double mean, double std);
double constrainAngle(double angle);
double angleDistance(double angle1, double angle2);
Vector3d forwardModel(Vector3d x, Vector3d u);
Vector3d reverseModel(Vector3d x, Vector3d u);
Vector3d forwardModelFrac(Vector3d x, Vector3d u, double alpha);
Vector3d reverseModelFrac(Vector3d x, Vector3d u, double alpha);
Vector3d inverseModel(Vector3d x1, Vector3d x0);
Matrix3d getInverseCovariance(Vector3d x, Vector3d u, Matrix3d Q);
Matrix3d getInverseCovariance(Vector3d u, Matrix3d Q);
double getForwardLogPx(Vector3d x, Vector3d xh, Matrix3d icov);
double getReverseLogPx(Vector3d x, Vector3d xh, Matrix3d icov);
double get2DForwardLogPx(Vector3d x, Vector3d xh, Matrix3d icov);
double get2DReverseLogPx(Vector3d x, Vector3d xh, Matrix3d icov);
double getForwardDPx(Vector3d x, Vector3d u, Vector3d xh, Matrix3d icov);
double getReverseDPx(Vector3d x, Vector3d u, Vector3d xh, Matrix3d icov);
double getForwardDPx_2(Vector3d x0, Vector3d x1, Vector3d xh, Matrix3d icov);
double getReverseDPx_2(Vector3d x0, Vector3d x1, Vector3d xh, Matrix3d icov);
double get2DForwardDPx_2(Vector3d x0, Vector3d x1, Vector3d xh, Matrix3d icov);
double get2DReverseDPx_2(Vector3d x0, Vector3d x1, Vector3d xh, Matrix3d icov);
double getRadius(Vector3d x);



#endif /* MOTIONMODEL_H_ */
